N94- 35646 


Motion Models in Attitude Estimation* 


D. Chu, Z. Wheeler, and J. Sedlak 
Computer Sciences Corporation (CSC) 
Lanham-Seabrook, Maryland, USA 


Abstract 

Attitude estimators use observations from different times to reduce the effects of noise, if the vehicle is rotating, the 
attitude at one time needs to be propagated to that at another time. If the vehicle measures its angular velocity, attitude 
propagation entails integrating a rotational kinematics equation only. If a measured angular velocity is not available, 
torques can be computed and an additional rotational dynamics equation integrated to give the angular velocity. 

Initial conditions for either of these integrations come from the estimation process. Sometimes additional quantities, such 
as gyro and torque parameters, are also solved for. Although the partial derivatives of attitude with respect to initial 
attitude and gyro parameters are well known, the corresponding partial derivatives with respect to initial angular velocity 
and torque parameters are less familiar. They can be derived and computed numerically in a way that is analogous to 
that used for the initial attitude and gyro parameters. 

Previous papers have demonstrated the feasibility of using dynamics models for attitude estimation but have not 
provided details of how ef x:h angular velocity and torque parameters can be estimated. This tutorial paper provides 
some of that detail, notab.y how to compute the state transition matrix when closed form expresions are not available, 
ft also attempts to put dynamics estimation in perspective by showing the progression from constant to gyro-propagated 
to dynamics-propagated attitude motion models. Readers not already familiar with attitude estimation will find this paper 
an introduction to the subject, and attitude specialists may appreciate the collection of heretofore scattered results 
brought together in a single place. 


•This work was supported by the National Aeronautics and Space Administration (NASA)/Goddard Space Flight Center 
(GSFC), Greenbeft, Maryland, under Contract NAS 5-31500. 
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Attitude Estimation 


Modeling attitude motion makes it possible to use observations taken at different attitudes over longer time 
spans. Having more data reduces the effect of noise and generally improves estimate accuracy. Estimation 
is explained by Sorenson (Ref. 1) but is reviewed here to establish notation. 

Least Squares 

To d etermin e some parameter of interest, such as attitude, the "best" least-squares value is the one that 
minimizes the squared difference between the observations and values computed from the state. 
Observations zj at times t t are a function of the state X and time t t but also include noise v t : 

z, = + vi 

If the noise has constant variance (second order stationary) and is uncorrelated in time (white), its 
autocorrelation is 

E(y t Vj T ) = - t) 

If each noise component is also independent, the matrix R is diagonal. The Dirac delta function 6(f) is 
defined to be zero everywhere but at the origin and to integrate to 1: 


o* 

f 6(t)dt = 1 

O' 

The correct value of the state should minimize the residuals that is, the difference between the 
observed and computed observation values: 


Az, = z, - h{x,t) 

The optimal value for the state is determined by minimizing a loss function defined as the sum of the 
squared observation residuals: 


Newton-Raphson Solution 


J = 


ifariVaS 

l ** 1 


Least-squares problems may be solved in several ways. The method used here is the Newton-Raphson 
method as described by Wertz (Ref. 2). It begins by taking the derivative of the loss function with respect 
to the state and setting it to zero: 


% - sVff, -8 

ax j,i 


where the derivative of the modeled observation with respect to the state is 
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If the observations are linear functions of the state, this equation can be solved as follows: 


u - SI f *£]" 

aziat 2 ) 

where ignoring the second derivative of observations with respect to the state — the second derivative of 
the loss function is 


This matrix is called the normal or information matrix and is the inverse of the covariance matrix P: 

p - 1 - 

i=i 

If the observations are nonlinear functions of the state, the solution is computed recursively: 

x - x + Ax 

This iteration is explic’* in the batch estimator, where all observations are processed together. It can be 
implicit in the sequent. estimator, where numerical convergence at a given time is combined with 
observability convergence with increasing time. 

If one already has a state estimate based on past information, that estimate can be updated to reflect 
additional observations without having to process all the observations over agaia The resultant solution!? 
is an average in which the two solutions are weighted by their respective information matrices: 

x - Pip;% * p 0 '^) = x, + pp-\x y - jy 

and the total information P 1 is equal to the sum of the information in the two sets of observations: 

p - 1 = Po 1 * p; 1 

Parameterizing Attitude 

To conform to the approach outlined above, the quantity being estimated must be expressed as a vector. 
For attitude, this means that the familiar matrix representation is inappropriate. Attitude could be expressed 
as a quaternion, but because the four components are not independent, the corresponding covariance matrix 
would be singular; it would not be invertible. 

Attitude can also be expressed in axis-angle or rotation vector form, where the three components are all 
independent (Ref. 2). Rotation vectors still have the unavoidable problem that finite rotations are not 
additive, they are very nonlinear. This problem can be circumvented by estimating the attitude error rather 

than the attitude itself. As long as the attitude error is small, the linear approximations made so far are 
justified. 
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The error vector in body coordinates A<? takes the true attitude A to the estimated attitude A . 

A' = R(A 3)A 

The rotation matrix J%A3) is given by the formula 

WAS) = / - sin(Afl) + [1 - cos(Afl)] 

where A a is the error magnitude, Ad is the error direction, and / is the 3 by 3 identity matrix. The tilde 
denotes the antisymmetric matrix 


a = 


0 

*3 

~<h 


-<h 

0 


«2 

-a. 


which is also the cross product operator 


a x b - db 


K^A 3) can also be expressed as 


R(A&) = cos(A a)I + [1 


- cos(Aa)]AAAd T - sin(Aa) 



by using the identity 


Ad 2 = A<i 2 (AAAd T - /) 

Once the state has been solved for and the attitude error vector obtained, the attitude matrix is updated 
with the rotation matrix 


A' - *(-A 3) A' 


Estimation With a Drift Model 


Modeling Motion 

Attitude may not remain constant over the data spans necessary to average out long-term errors. If 
differential equations can be written to model the attitude motion, observations may be predicted more 
accurately than with a constant attitude model. As in Gelb (Ref. 3), these equations are linearized to make 
them easier to solve: 

£ = F(jf,0* + G%t)V 
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Here, w is a noise term reflecting that the uncertainty is the state propagation. Even though these 
equations may not be exact, their solution, in particular the state transition matrix ® , makes it possible 
to propagate the state from the past to the present. 

If the attitude drifts at random, it can be modeled as the Maikov process 

dLS . A* , 
dt t 

where t is a characteristic time for the drift. The larger t is, the larger the variance. As t increases, the 
Markov model approaches the random walk model 


Process Noise 

Whatever the motion model, the state cannot be propagated with certainty. Farrenkopf (Ref. 4) gives a 
very clear discussion of this "process noise," and Lefferts et al. (Ref. 5) demonstrate how it can 
reflected in the covariance matrix for the sequential estimator. The propagation covariance A P can 
defined as 


A P = E[x(o)*(t) t ]U 


and evaluated by substituting for x : 


ft W \ T 

J G(x)w(x)dx f G(o)w(o)do 

o J \o 


IV 


AP = £ 

If the components of w are independent, white, and stationary, the process noise is 


£[w(t)w t (o)] = Q A(o - t) 

where Q is diagonal. This expression can be simplified by making the two integrals a double integral and 
by bringing the expectation operator inside. The result is 


AP = JV?(tX2 G T (x)dx 
o 

After propagation, the total covariance is just the sum of the propagated covariance and the process noise 
contribution: 

P - ®P® T + AP 
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Estimation With Gyro Propagation 


Attitude Error Equation 

The attitude error equation is derived by perturbing the kinematic equation for attitude matrix propagation 

“--&A 

dt 

(Ref. 6). Given the angular velocity 3 and initial attitude, one can integrate to find the attitude at any 
later time. A small change in the attitude matrix can be approximated as 

A' - (/ - Aa)A 

Error can be introduced into the true angular velocity 3 as 

5 / = 3 + A3 

Substituting these expressions into the kinematic equation and subtracting the unperturbed equation gives 

-—A - A d— = (3 Ad - A3 + A3Ad)A 
dtdt 

Ignoring errors that are the product of two small quantities gives 

— A + A d^ = (-3 Ad + A3 )A 
dt dt 


Substituting for — and postmultiplying by A T gives 
dt 


dAd 

dt 


AaG> = -3Ad + A3 


Isolating 


dAd 

dt 


and postmultiplying by A3 gives 


^■A 3 = (Ad3 + A 3) Ad 

dt 


Because AdAd is identically zero, its derivative is zero as well: 


Ad— = -Ad3Ad - A 3 Ad 

dt 
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Changin g multiplication order in the first term on the right and factoring out the matrix A a gives the 
attitude error equation 


— = -SA 5 + AS 
dt 


Modeling Gyro Parameters 

Gyro errors are ascribed to parameters, including biases b g , scale factor errors k g , misalignments M g , and 
noise w y : 


AS = G b b + G t (S)£ + G m (S)n i + w v 


Like attitude drift, gyro parameter drift A g can be modeled as a Markov process plus white noise. To 
estimate some set of gyro parameters g, the state equations become 


d_ A5 
dt Ag 


S G ■ 



t 


A 5 

^ | !-• 

1 

o 


. *8 . 


10 


w v ' 

0 / 


. V5 «. 


where the matrix G g is constructed such that the modeled angular velocity error is 


AS = G g Ag 


For batch estimation, the transition matrix also serves as the partial derivative of current state variables 
with respect to their epoch values. If the transition matrix has the form 

_ [ 4> * 

© = 

[o X ] 

the derivatives of the current attitude error with respect to the epoch attitude error A 3^ and gyro parameter 
errors Ag are 


3Ag 

dA 


l« ( = 4>(Vo) 


dA3 . 
dA g'~< 


■ 


After the gyro parameter errors have been solved for, the gyro parameters are updated as 


i ~ 2 - Ag 
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Estimation With Torque Modeling 


Angular Velocity Error Equation 

Harvie (Ref. 7) has demonstrated the feasibility of accurately modeling torques for attitude propagation, 
but there he estimates angular velocity bias rather than epoch angular velocity. The following development 
yields the partial derivatives for epoch angular velocity. 

Given accurate torque models, the angular velocity can be found by integrating Euler’s equation for 
rotational motion 

N = G> x£ + — 
dt 

where if is the external torque and L is the total angular momentum both expressed in the rotating body 
frame. The angular momentum is composed of two parts, one due to the rotation of the body with its 
inertia tensor J and one due to any internal angular momentum h 

£ = JQ + X 

To reduce the possibility of confusion over units, the angular acceleration 3 can replace the torque as the 
state variable 

3 =;■'!? 

to give the differential equation for the angular velocity as 


5 x (75 + ft) + — 
dt 

Along with the initial attitude, the integrated angular velocity now serves as input to the kinematic 
propagation equation. An error in the predicted angular acceleration A3 causes a corresponding error in 
the angular velocity A <3 : 



d(5 + AS) 
dt 


|(« 


= (3 + AS) - J' 1 < (6 + Au) [ 7(5 + A5) 



Subtracting the original differential equation and discarding terms that are the product of two small 
quantities leaves 

— = Aa - J' 1 ( 67 A 6 + A 676 + A 6 A ) 
dt 

Reversing multiplication order in the second and third terms in parentheses and factoring out A 5 gives 


<fA5 

dt 


= 7 _1 (-67 + L) A6 + A3 


The corrected angular velocity is computed as 


5 *■ 6 — Au 
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Modeling Dynamics Parameters 


As for angular velocity errors, angular acceleration errors can be attributed to poorly known dynamics 
parameters such as the moments J u , the products of inertia , the residual magnetic dipoles M r , and 
noise w v : 


AS • J ' ( GjAJ m * Gj'AJy - C„ A*, ) 


w.. 


To estimate some set of dynamics parameter errors A <7, the state equations become 


d_ 

dt 


a a 


A 3 

= 

a<7 



-fij 

0 

0 


I 

0 


0 

/ 


A3 


0 0 0 


5 

Ad 

+ 

0/0 


*v 

a a 


0 0/ 


. . 


where G d comes from the coefficient matrices above such that the modeled angular acceleration is 


a a = G d aJ 

If the state transition matrix has the form 


$ 


4> C l 

0 v v 

0 0k 


the derivatives of the current attitude error with respect to the initial angular velocity error At3 0 and 
dynamics parameter error A J are 


3A 3 , 
3Ac> 0 U ' 


C(Vo) 


8A a , 
3AJ U ‘‘ 


5(t P f 0 ) 


When the dynamics parameter errors are found, the dynamics parameters are corrected as 


<7 *- <7- a<7 


Computing the State Transition Matrix 


Short Time Steps 

The state transition matrix is needed to chain derivatives back to epoch and to propagate the covariance 
matrix. Brogan (Ref. 8) discusses its evaluation, as do other texts on linear system theory. If the 
coefficient matrix F is constant over the time step, the transition matrix is simply the exponential of the 
product of the coefficient matrix and the time step: 
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®Mo) = e 




For simple coefficient matrices, a closed form solution can sometimes be obtained by simplifying the 
infinite series 


= / + tF + — F 2 + — F 3 + 
2 3! 



Harvie (Ref. 7) simply tnmcated the series without ill effect With this approach, one always wonders how 
many terms to include. If die coefficient matrix has all zeros on the main diagonal, the infinite series 
terminates after n or fewer terms. Writing the matrix as the sum of an upper U and a lower triangular 
matrix L and using the binomial theorem to express powers of their sum 


(U + L) H 


A w! U r L H r 
h r\{n - r)! 


and substituting into the series 


.Ft 


t* * 

E^E 


n! 


U r\(n - r)\ 


U r L f 


the infinite series terminates after at most n terms, since the triangular matrices U and L are nilpotent. 

Exact formulas exist which involve only moderate additional effort. Using the Cayley-Hamilton theorem, 
it can be shown that the exponential of an n by n matrix is equal to the n - 1 order polynomial in tF: 

= aj + ttjf F + a 2 t 2 F 2 + . . . + 

where the a t are scalar coefficients that are determined by solving a set of linear equations. This is the 
form of the attitude propagation matrix given above. This equation shows that the reduction to three terms 
is not due to the antisymmetric nature of the coefficient matrix. 

The coefficients a t come from a system of equations of the form 

e Xf = a 0 + a x kf + a. 2 k]t 2 + . . . + 

where the X i are the eigenvalues of F. If repeated eigenvalues exist, the repeated equation can be 
differentiated with respect to the eigenvalue to give an additional equation. 



Another eigenvalue approach exponentiates the diagonal matrix whose elements are the eigenvalues of Ft: 


.V 


0 


e 


A t x 


0 

I 

0 


e 


V 


s 


0 


0 

0 


e * 


We then perform a similarity transformation using matrices formed from the eigenvectors 

e n = Se^S' 1 

where the S matrix is made up of the eigenvectors v^: 

S = (v,:v 2 : ... :V B ) 


Again, when a full set of eigenvalues does not exist, the diagonal matrix becomes block diagonal, and 
generalized eigenvectors must be found. 

Closed Form Expressions 

Solutions for the attituue error A 3 can be written in terms of the transition submatrices 4> and ijr as 

AA(f) = 4>(f,0) A5(0) + 4r(f,0) Ao(0) 


A closed form expression for the transition matrix 4> can be obtained directly fiom the infinite series 
definition by collapsing the series with the identity 

- 3 2 - 


or by using the Cayley-Hamilton eigenvalue method outlined above. The eigenvalues X, of the 
antisymmetric matrix are 0 and ±iut. The three equations for the a i become 

,o _ „ 

e - a 0 

e ia< = a 0 + Oj (iur) + a 2 (io>r ) 2 
e’ lM = a 0 + Ojf-ioir) + a 2 (-ic»f ) 2 

giving the transition submatrix $ as 


4>(f,o> = i - 


sin 



+ (1 - cos tor) 
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In addition to propagating the attitude error using angular velocity, the same expressions can be used to 
propagate the angular velocity error for an axisymmetric spinning spacecraft using the angular acceleration 
error. 

The transition submatrix «|r for the inhomogeneous solution is the convolution of the transition matrix $ 
found above with the "forcing function” coefficient matrix G x : 

i 

MO) = / 4>(r,x) G x (x) dx 
0 

and for constant G x is equal to 

Recursion Relations 

For batch estimation, the transition matrix must be accumulated over the length of the batch. Over such 
long times, the assumption that the coefficient matrix F remains constant may not be valid. In this case, 
the transition matrix can be computed recursively: 

where the initial value of the $ is the identity matrix 

® (*o**o) = I 

Because the batch estimator needs derivatives of the attitude error only, it is not necessary to form the 
entire transition matrix. The submatrices can also be computed recursively. For gyro propagation, the 
recursion relations are 

*(W = ♦(M-iWWo) + 

where the initial values of these matrices are 

~ I 

»K*o**o) = 0 

For dynamic propagation, the attitude error recursion is unchanged, but two additional recursion relations 
exist: 

tMo) = ♦<***-! W-W + C(f < ,t i - l )v(r i . 1 ,r 0 ) 
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where these matrices are initially zero: 


C<Wo> = *<Vo) = 0 

Summary 

Modeling attitude motion can improve accuracy by allowing the estimate to follow the changing state. The 
model may be random, as in Maikov drift; deterministic, as in gyro propagation; or a combination of both 
types. Differential equations for the attitude error provide a means for propagating covariance in the 
sequential estimator and for chaining derivatives back to epoch in the batch estimator. Closed form 
expressions are available for the state transition matrix solutions to those equations, and the state can be 
augmented to include propagation parameters, such as gyro biases. 

These methods reflect attitude estimation as traditionally practiced by the NASA/Goddard Space Right 
Center Right Dynamics Facility. The recent addition of dynamics motion models has required new 
expressions for the derivatives of the attitude with respect to dynamics parameters, such as products of 
inertia, and numerical evaluation of the state transition matrices. It is hoped that the expressions for these 
derivatives, the transition matrix methods, and unified treatment of motion models provided here will be 
useful to those who follow in the practice of attitude estimation. 
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